#include "bsp_motor.h"
#include "timer.h"
#include "tcxo.h"
#include "gpio.h"
#include "chip_core_irq.h" // 芯片核心中断处理
#include "app_init.h"      // 应用初始化接口
#include "common_def.h"    // 通用宏定义
#include "soc_osal.h"      // 操作系统抽象层
#include "osal_debug.h"    // 调试输出接口
#include "cmsis_os2.h"     // CMSIS-RTOS2接口
#include "app_init.h"      // 应用初始化接口
#include "chip_core_irq.h" // 芯片核心中断处理
#include "PID.h"
// PWM通道组定义
#define PWM_GROUP_ID 0
uint8_t channel_group[2] = {MOTOR1_PWM_CHANNEL, MOTOR2_PWM_CHANNEL};

/**
 * @brief  初始化电机设备。
 * @param   无
 * @retval ERRCODE_SUCC 成功。
 * @retval Other        失败，参考 @ref errcode_t 。
 */
errcode_t bsp_motor_init(void)
{
    // 初始化电机GPIO引脚
    uapi_pin_set_mode(MOTOR1_GPIO_IN1, PIN_MODE_0); // 电机1-IN1
    uapi_gpio_set_dir(MOTOR1_GPIO_IN1, GPIO_DIRECTION_OUTPUT);
    uapi_pin_set_mode(MOTOR1_GPIO_IN2, PIN_MODE_0); // 电机1-IN2
    uapi_gpio_set_dir(MOTOR1_GPIO_IN2, GPIO_DIRECTION_OUTPUT);
    uapi_pin_set_mode(MOTOR2_GPIO_IN1, PIN_MODE_0); // 电机2-IN1
    uapi_gpio_set_dir(MOTOR2_GPIO_IN1, GPIO_DIRECTION_OUTPUT);
    uapi_pin_set_mode(MOTOR2_GPIO_IN2, PIN_MODE_0); // 电机2-IN2
    uapi_gpio_set_dir(MOTOR2_GPIO_IN2, GPIO_DIRECTION_OUTPUT);

    // 初始化电机PWM引脚
    uapi_pwm_init();
    uapi_pin_set_mode(MOTOR1_GPIO_PWM, PIN_MODE_1);
    uapi_pin_set_mode(MOTOR2_GPIO_PWM, PIN_MODE_1);

    // 配置PWM组参数
    uapi_pwm_set_group(PWM_GROUP_ID, channel_group, 2);

    bsp_motor_stop(); // 初始化时停止电机

    return ERRCODE_SUCC;
}

/**
 * @brief  控制电机停止
 * @param  无
 * @retval ERRCODE_SUCC 成功。
 * @retval Other        失败，参考 @ref errcode_t 。
 */
errcode_t bsp_motor_stop(void)
{
    uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_LOW);

    // 关闭PWM，重新启动
    uapi_gpio_set_val(MOTOR1_GPIO_PWM, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR2_GPIO_PWM, GPIO_LEVEL_LOW);

    uapi_pwm_close(MOTOR1_PWM_CHANNEL);
    uapi_pwm_close(MOTOR2_PWM_CHANNEL);

    return ERRCODE_SUCC;
}

/**
 * @brief  控制电机前进
 * @param    speed 电机速度。 范围0-100, 0为停止, 100为最大速度
 * @retval ERRCODE_SUCC 成功。
 * @retval Other        失败，参考 @ref errcode_t 。
 */
errcode_t bsp_motor_advance(uint16_t speed)
{
    if (speed == 0 || speed > 100)
    {
        osal_printk("bsp_motor_advance, The speed cannot be 0 or exceed 100.\n");
        return ERRCODE_INVALID_PARAM;
    }
    uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_LOW);

    // 关闭PWM，重新启动
    uapi_pwm_close(MOTOR1_PWM_CHANNEL);
    uapi_pwm_close(MOTOR2_PWM_CHANNEL);

    // 配置PWM参数
    pwm_config_t cfg = {
        .low_time = 100 - speed,
        .high_time = speed,
        .offset_time = 0,
        .cycles = 0,
        .repeat = true};
    if (uapi_pwm_open(MOTOR1_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }
    if (uapi_pwm_open(MOTOR2_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }

    // 启动PWM
    uapi_pwm_start(MOTOR1_PWM_CHANNEL);
    uapi_pwm_start(MOTOR2_PWM_CHANNEL);
    uapi_pwm_start_group(PWM_GROUP_ID);

    return ERRCODE_SUCC;
}

/**
 * @brief  控制电机后退
 * @param    speed 电机速度。 范围0-100, 0为停止, 100为最大速度
 * @retval ERRCODE_SUCC 成功。
 * @retval Other        失败，参考 @ref errcode_t 。
 */
errcode_t bsp_motor_retreat(uint16_t speed)
{
    if (speed == 0 || speed > 100)
    {
        osal_printk("bsp_motor_advance, The speed cannot be 0 or exceed 100.\n");
        return ERRCODE_INVALID_PARAM;
    }

    uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_HIGH);

    // 关闭PWM，重新启动
    uapi_pwm_close(MOTOR1_PWM_CHANNEL);
    uapi_pwm_close(MOTOR2_PWM_CHANNEL);

    // 配置PWM参数
    pwm_config_t cfg = {
        .low_time = 100 - speed,
        .high_time = speed,
        .offset_time = 0,
        .cycles = 0,
        .repeat = true};
    if (uapi_pwm_open(MOTOR1_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }
    if (uapi_pwm_open(MOTOR2_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }

    // 启动PWM
    uapi_pwm_start(MOTOR1_PWM_CHANNEL);
    uapi_pwm_start(MOTOR2_PWM_CHANNEL);
    uapi_pwm_start_group(PWM_GROUP_ID);

    return ERRCODE_SUCC;
}

/**
 * @brief  控制电机左前方运行
 * @param    speed 电机速度。 范围0-100, 0为停止, 100为最大速度
 * @retval ERRCODE_SUCC 成功。
 * @retval Other        失败，参考 @ref errcode_t 。
 */
errcode_t bsp_motor_front_left(uint16_t speed)
{
    if (speed == 0 || speed > 100)
    {
        osal_printk("bsp_motor_advance, The speed cannot be 0 or exceed 100.\n");
        return ERRCODE_INVALID_PARAM;
    }

    uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_LOW);

    // 关闭PWM，重新启动
    uapi_pwm_close(MOTOR1_PWM_CHANNEL);
    uapi_pwm_close(MOTOR2_PWM_CHANNEL);

    // 配置PWM参数
    pwm_config_t cfg = {
        .low_time = 100 - speed,
        .high_time = speed,
        .offset_time = 0,
        .cycles = 0,
        .repeat = true};
    if (uapi_pwm_open(MOTOR1_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }
    if (uapi_pwm_open(MOTOR2_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }

    // 启动PWM
    uapi_pwm_start(MOTOR1_PWM_CHANNEL);
    uapi_pwm_start(MOTOR2_PWM_CHANNEL);
    uapi_pwm_start_group(PWM_GROUP_ID);

    return ERRCODE_SUCC;
}

/**
 * @brief  控制电机右前方运行
 * @param    speed 电机速度。 范围0-100, 0为停止, 100为最大速度
 * @retval ERRCODE_SUCC 成功。
 * @retval Other        失败，参考 @ref errcode_t 。
 */
errcode_t bsp_motor_front_right(uint16_t speed)
{
    if (speed == 0 || speed > 100)
    {
        osal_printk("bsp_motor_advance, The speed cannot be 0 or exceed 100.\n");
        return ERRCODE_INVALID_PARAM;
    }

    uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_HIGH);
    uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_LOW);
    uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_HIGH);

    // 关闭PWM，重新启动
    uapi_pwm_close(MOTOR1_PWM_CHANNEL);
    uapi_pwm_close(MOTOR2_PWM_CHANNEL);

    // 配置PWM参数
    pwm_config_t cfg = {
        .low_time = 100 - speed,
        .high_time = speed,
        .offset_time = 0,
        .cycles = 0,
        .repeat = true};
    if (uapi_pwm_open(MOTOR1_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }
    if (uapi_pwm_open(MOTOR2_PWM_CHANNEL, &cfg) != ERRCODE_SUCC)
    {
        return ERRCODE_PWM_NOT_OPEN;
    }

    // 启动PWM
    uapi_pwm_start(MOTOR1_PWM_CHANNEL);
    uapi_pwm_start(MOTOR2_PWM_CHANNEL);
    uapi_pwm_start_group(PWM_GROUP_ID);

    return ERRCODE_SUCC;
}

uint32_t ext_clock_count = 0;

// 外部时钟信号中断处理
void ext_clock_isr(pin_t pin, uintptr_t param)
{
    unused(param);
    ext_clock_count++;              // 外部时钟脉冲计数
    uapi_gpio_clear_interrupt(pin); // 清除中断标志
}

uint32_t get_ext_clock_count(void)
{
    uint32_t temp = ext_clock_count / 2;
    return temp;
}

void encoder_init(pin_t A, pin_t B, gpio_callback_t encoder)
{
    uapi_timer_init();
    uapi_gpio_init();
    uapi_pin_set_mode(A, PIN_MODE_0);
    uapi_gpio_set_dir(A, GPIO_DIRECTION_INPUT);

    uapi_pin_set_mode(B, PIN_MODE_0);
    uapi_gpio_set_dir(B, GPIO_DIRECTION_INPUT);

    // 注册上升沿中断
    uapi_gpio_register_isr_func(A, GPIO_INTERRUPT_RISING_EDGE, encoder);
    uapi_gpio_register_isr_func(B, GPIO_INTERRUPT_RISING_EDGE, encoder);
    // 使能中断
    uapi_gpio_enable_interrupt(A);
    uapi_gpio_enable_interrupt(B);
}

void pwm_out(int left, int right)
{
    // 关闭PWM，重新启动
    uapi_pwm_close(MOTOR1_PWM_CHANNEL);
    uapi_pwm_close(MOTOR2_PWM_CHANNEL);
    pwm_config_t cfg_left = {
        .low_time = 10000 - left,
        .high_time = left,
        .offset_time = 0,
        .cycles = 0,
        .repeat = true};
    pwm_config_t cfg_right = {
        .low_time = 10000 - right,
        .high_time = right,
        .offset_time = 0,
        .cycles = 0,
        .repeat = true};
    if (left > 0 && right > 0)
    {
        uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_HIGH);
        uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_LOW);
        uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_LOW);
        uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_HIGH);
        uapi_pwm_open(MOTOR1_PWM_CHANNEL, &cfg_left);
        uapi_pwm_open(MOTOR2_PWM_CHANNEL, &cfg_right);
        // 启动PWM
        uapi_pwm_start(MOTOR1_PWM_CHANNEL);
        uapi_pwm_start(MOTOR2_PWM_CHANNEL);
        uapi_pwm_start_group(PWM_GROUP_ID);
    }
    // else
    // {
    //     uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_LOW);
    //     uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_HIGH);
    //     uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_LOW);
    //     uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_HIGH);
    // }
}
void pwm_test(pin_t pin, uint8_t pwm_x, double duty_cycle)
{
    uint32_t temp;
    if (duty_cycle > 0)
    {
        temp = 100 * duty_cycle; // 5000
    }
    else
    {
        temp = 100 * -duty_cycle; // 5000
    }
    uapi_pwm_close(pwm_x);
    pwm_config_t cfg_left = {
        .low_time = 10000 - temp,
        .high_time = temp,
        .offset_time = 0,
        .cycles = 0,
        .repeat = true};
    if (PWM_0 == pwm_x && duty_cycle)
    {
        uapi_gpio_set_val(MOTOR1_GPIO_IN1, duty_cycle > 0);
        uapi_gpio_set_val(MOTOR1_GPIO_IN2, !(duty_cycle > 0));
    }
    else if (PWM_4 == pwm_x && duty_cycle)
    {
        uapi_gpio_set_val(MOTOR2_GPIO_IN1, duty_cycle > 0);
        uapi_gpio_set_val(MOTOR2_GPIO_IN2, !(duty_cycle > 0));
    }

    if (duty_cycle == 0)
    {
        uapi_gpio_set_val(MOTOR1_GPIO_IN1, GPIO_LEVEL_LOW);
        uapi_gpio_set_val(MOTOR1_GPIO_IN2, GPIO_LEVEL_LOW);
        uapi_gpio_set_val(MOTOR2_GPIO_IN1, GPIO_LEVEL_LOW);
        uapi_gpio_set_val(MOTOR2_GPIO_IN2, GPIO_LEVEL_LOW);

        // 关闭PWM，重新启动
        uapi_pwm_close(MOTOR1_PWM_CHANNEL);
        uapi_pwm_close(MOTOR2_PWM_CHANNEL);
    }
    else
    {
        uapi_pwm_open(pwm_x, &cfg_left);
        uapi_pwm_start(pwm_x);
        uint8_t channel_id = pwm_x;
        uapi_pwm_set_group(0, &channel_id, 1);
        uapi_pwm_start_group(0);
    }
}

// void sg90_init(void)
// {
//     uapi_pin_set_mode(GPIO_05, PIN_MODE_1);
//     uapi_pwm_close(PWM_5);
// }
// void sg90_angle(uint8_t angle)
// {
//     uapi_pwm_close(PWM_5);
//     uint32_t temp = (111.1112 * angle) + 5000;
//     // 5000
//     pwm_config_t cfg_left = {
//         .low_time = 6000,
//         .high_time = 6000,
//         .offset_time = 0,
//         .cycles = 0,
//         .repeat = true};
//     uapi_pwm_open(PWM_5, &cfg_left);
//     uapi_pwm_start(PWM_5);
//     uint8_t channel_id = PWM_5;
//     uapi_pwm_set_group(0, &channel_id, 1);
//     uapi_pwm_start_group(0);
//     uapi_pwm_get_frequency(PWM_5);
// }
